#include <Wire.h>
#include "SSD1306Ascii.h"
#include "SSD1306AsciiWire.h"
#include "Fonts/SystemFont5x7.h" // 引入小字体
#include <Adafruit_INA219.h>

#define I2C_ADDRESS 0x3C
#define RST_PIN -11111
SSD1306AsciiWire oled;

Adafruit_INA219 ina219;

// 锂电池参数
const float MAX_BATTERY_VOLTAGE = 4.2;
const float MIN_BATTERY_VOLTAGE = 3.0;

// 滑动平均滤波参数
#define FILTER_SIZE 5
float voltageBuffer[FILTER_SIZE];
int bufferIndex = 0;

// 锂电池电压-电量非线性映射表
float calculateBatteryPercentage(float voltage) {
  if (voltage >= 4.18) return 100;  // 接近满电
  if (voltage >= 4.0)  return 90;   // 高电量
  if (voltage >= 3.85) return 75;   // 典型使用区间
  if (voltage >= 3.75) return 60;   // 中高电量
  if (voltage >= 3.65) return 40;   // 中等电量
  if (voltage >= 3.55) return 25;   // 低电量
  if (voltage >= 3.4)  return 10;   // 极低电量
  return 0;                         // 放空
}

// 电压滤波函数
float filterVoltage(float newVoltage) {
  voltageBuffer[bufferIndex++] = newVoltage;
  if (bufferIndex >= FILTER_SIZE) bufferIndex = 0;
  
  float sum = 0;
  for (int i = 0; i < FILTER_SIZE; i++) {
    sum += voltageBuffer[i];
  }
  return sum / FILTER_SIZE;
}

void setup() {
  Wire.begin();
  Wire.setClock(400000L);
  
  #if RST_PIN >= 0
    oled.begin(&Adafruit128x64, I2C_ADDRESS, RST_PIN);
  #else
    oled.begin(&Adafruit128x64, I2C_ADDRESS);
  #endif
  
  oled.clear();
  
  // 初始化滤波缓冲区
  for (int i = 0; i < FILTER_SIZE; i++) {
    voltageBuffer[i] = 0;
  }
  
  // 初始化INA219
  if (!ina219.begin()) {
    oled.setFont(System5x7); // 使用小字体显示错误信息
    oled.setCursor(0, 0);
    oled.print("INA219 Init Fail!");
    while (1);
  }
  ina219.setCalibration_32V_2A();
  oled.setFont(System5x7); // 使用小字体
  oled.setCursor(0, 0);
  oled.print("INA219 Init OK!");
  delay(1000);
  oled.clear();
}

void loop() {
  // 读取原始数据
  float rawVoltage = ina219.getBusVoltage_V();
  float current = ina219.getCurrent_mA();
  float walt = ina219.getPower_mW();
  
  // 滤波处理
  float filteredVoltage = filterVoltage(rawVoltage);
  
  // 计算电量百分比
  float batteryPercentage = calculateBatteryPercentage(filteredVoltage);
  
  // OLED显示
  oled.setFont(System5x7); // 使用小字体
  
  // 功率显示
  oled.setCursor(0, 0);
  oled.print("Walt: ");
  oled.print(walt, 2);
  oled.print(" mW");

  // 电压显示
  oled.setCursor(0, 2);
  oled.print("Voltage: ");
  oled.print(filteredVoltage, 2);
  oled.print(" V");
  
  // 电流显示
  oled.setCursor(0, 4);
  oled.print("Current: ");
  oled.print(current, 1);
  oled.print(" mA");
  
  // 电量显示
  oled.setCursor(0, 6);
  oled.print("Battery: ");
  oled.print(batteryPercentage, 0);
  oled.print(" %");
  
  delay(1000);
  oled.clear();
}